/** ============================================================================
 *  @file   tskMessage.c
 *
 *  @path   
 *
 *  @desc   This is simple TSK based application that uses MSGQ. It receives
 *          and transmits Matrixes from/to the GPP and runs the DSP
 *          application code (located in an external source file)
 *
 *  @ver    1.10
 */


/*  ----------------------------------- DSP/BIOS Headers            */
#include "MatrixDSPcfg.h"
#include <gbl.h>
#include <sys.h>
#include <sem.h>
#include <msgq.h>
#include <pool.h>

/*  ----------------------------------- DSP/BIOS LINK Headers       */
#include <dsplink.h>
#include <platform.h>
#include <failure.h>

/*  ----------------------------------- Sample Headers              */
#include <MatrixDSP_config.h>
#include <tskMessage.h>

#ifdef __cplusplus
extern "C" {
#endif


/* FILEID is used by SET_FAILURE_REASON macro. */
#define FILEID  FID_APP_C


#define SIZE_MAX_MATRIX 128
#define SIZE_PACKAGE_ENTRIES    50
#define SIZE_CHARLIST   SIZE_MAX_MATRIX*SIZE_MAX_MATRIX*4+1
#define COMMAND_CONTINUE        0x55
#define COMMAND_TERMINATE       0xAA

ControlMsg Gmsg;
Uint32 mat1[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX],mat2[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX],prod[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX];
Uint16 h,i,j,k;

TSKMESSAGE_TransferInfo* info = NULL;

/* Place holder for the MSGQ name created on DSP */
Uint8 dspMsgQName[DSP_MAX_STRLEN];

/* Number of iterations message transfers to be done by the application. */
extern Uint16 matrixSize;


/** ============================================================================
 *  @func   TSKMESSAGE_create
 *
 *  @desc   Create phase function for the TSKMESSAGE application. Initializes
 *          the TSKMESSAGE_TransferInfo structure with the information that will
 *          be used by the other phases of the application.
 *
 *  @modif  None.
 *  ============================================================================
 */
Int TSKMESSAGE_create(TSKMESSAGE_TransferInfo** infoPtr)
{
    Int status = SYS_OK;
    MSGQ_Attrs msgqAttrs = MSGQ_ATTRS;
    MSGQ_LocateAttrs syncLocateAttrs;

    /* Allocate TSKMESSAGE_TransferInfo structure that will be initialized
     * and passed to other phases of the application */
    *infoPtr = MEM_calloc(DSPLINK_SEGID, sizeof(TSKMESSAGE_TransferInfo), DSPLINK_BUF_ALIGN);
    if (*infoPtr == NULL)
    {
        status = SYS_EALLOC;
        SET_FAILURE_REASON(status);
    }
    else
    {
        info = *infoPtr;
        info->matrixSize = matrixSize;
        info->localMsgq = MSGQ_INVALIDMSGQ;
        info->locatedMsgq = MSGQ_INVALIDMSGQ;
    }

    if (status == SYS_OK)
    {
        /* Set the semaphore to a known state. */
        SEM_new(&(info->notifySemObj), 0);

        /* Fill in the attributes for this message queue. */
        msgqAttrs.notifyHandle = &(info->notifySemObj);
        msgqAttrs.pend = (MSGQ_Pend) SEM_pendBinary;
        msgqAttrs.post = (MSGQ_Post) SEM_postBinary;

        SYS_sprintf((Char *)dspMsgQName, "%s%d", DSP_MSGQNAME, GBL_getProcId());

        /* Creating message queue */
        status = MSGQ_open((String)dspMsgQName, &info->localMsgq, &msgqAttrs);
        if (status != SYS_OK)
        {
            SET_FAILURE_REASON(status);
        }
        else
        {
            /* Set the message queue that will receive any async. errors. */
            MSGQ_setErrorHandler(info->localMsgq, SAMPLE_POOL_ID);

            /* Synchronous locate.                           */
            /* Wait for the initial startup message from GPP */
            status = SYS_ENOTFOUND;
            while ((status == SYS_ENOTFOUND) || (status == SYS_ENODEV))
            {
                syncLocateAttrs.timeout = SYS_FOREVER;
                status = MSGQ_locate(GPP_MSGQNAME, &info->locatedMsgq, &syncLocateAttrs);
                if ((status == SYS_ENOTFOUND) || (status == SYS_ENODEV))
                {
                    TSK_sleep(1000);
                }
                else if(status != SYS_OK)
                {
#if !defined (LOG_COMPONENT)
                    LOG_printf(&trace, "MSGQ_locate (msgqOut) failed. Status = 0x%x\n", status);
#endif
                }
            }
        }
       /* Initialize the sequenceNumber */
        info->sequenceNumber = 0;
    }

    return status;
}


/** ============================================================================
 *  @func   SendAcknowledgement
 *
 *  @desc   This function will send a received acknowledgement to ARM.
 *          It is used by ReceiveMatrix to inform the ARM with the remaining elements
 *          of a Matrix. 
 *
 *  @modif  None
 *  ============================================================================
 */
void SendAcknowledgement(ControlMsg *msg, Uint32 ExpectedRequest){
    // Edit the message and send it over
    Int status = SYS_OK;
    
    /* Increment the sequenceNumber for next received message */
    info->sequenceNumber++;
    /* Make sure that sequenceNumber stays within the range of iterations */
    if (info->sequenceNumber == MSGQ_INTERNALIDSSTART)
    {
        info->sequenceNumber = 0;
    }
    MSGQ_setMsgId((MSGQ_Msg) msg, info->sequenceNumber);
    MSGQ_setSrcQueue((MSGQ_Msg) msg, info->localMsgq);

    msg->PackageEntries = ExpectedRequest;
    
    status = MSGQ_put(info->locatedMsgq, (MSGQ_Msg) msg);
    if (status != SYS_OK)
    {
        /* Must free the message */
        MSGQ_free ((MSGQ_Msg) msg);
        SET_FAILURE_REASON(status);
    }    
}

/** ============================================================================
 *  @func   RetrieveData
 *
 *  @desc   This function is used by ReceiveMatrix to retrieve Matrix 
 *          information stored in the message.
 *
 *  @ret    It returns the number of retrieved element of Matrix.
 *
 *  @modif  None
 *  ============================================================================
 */
Uint16 RetrieveData(ControlMsg *msg, Uint32 CurrMatrix[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX], Uint32 MatrixSize, Uint16 *Row, Uint16 *Colume){
    Uint16 CurrEntries = msg->PackageEntries;
    Uint16 RetrievedEntry = 0;
    
    for(RetrievedEntry=0; RetrievedEntry!=CurrEntries; RetrievedEntry++){   
        CurrMatrix[(*Row)][(*Colume)] = msg->int_arg[RetrievedEntry];
        
        (*Colume)++;
        if((*Colume)==MatrixSize){
            (*Colume)=0;
            (*Row)++;
        }
    }
    return RetrievedEntry;
}

/** ============================================================================
 *  @func   WaitMatrixMsg
 *
 *  @desc   This function will wait for a message from DSP.
 *
 *  @modif  None
 *  ============================================================================
 */
void WaitMatrixMsg(ControlMsg *msg){
    // Waiting for incoming message  
    Int status = MSGQ_get(info->localMsgq, (MSGQ_Msg*) &msg, SYS_FOREVER);
    if (status != SYS_OK)
    {
        MSGQ_free ((MSGQ_Msg) msg);
        SET_FAILURE_REASON(status);
    }
}

/** ============================================================================
 *  @func   ReceiveMatrix
 *
 *  @desc   This function will receive a Matrix which is sent by DSP.
 *
 *  @modif  None
 *  ============================================================================
 */
void ReceiveMatrix(Uint32 CurrMatrix[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX], Uint32 MatrixSize, ControlMsg *msg){
    Uint32 RemainingEnties = MatrixSize*MatrixSize;
    Uint32 CurrEntries=0;
    Uint16 Row=0, Colume=0;
    //Char8 *Data;
    
    while(RemainingEnties){
        WaitMatrixMsg(msg);
        CurrEntries = RetrieveData(msg, CurrMatrix, MatrixSize, &Row, &Colume);
        
        RemainingEnties-=CurrEntries;
        SendAcknowledgement(msg, RemainingEnties);
    }
}

/** ============================================================================
 *  @func   matMult
 *
 *  @desc   This function perform the multiplication of 2 matrixes.
 *
 *  @modif  None
 *  ============================================================================
 */
void matMult(Uint32 mat1[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX], Uint32 mat2[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX], Uint32 prod[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX], Uint32 matrixSize)
{
    Uint32 i, j, k;
    for (i = 0;i < matrixSize; i++)
    {
        for (j = 0; j < matrixSize; j++)
        {
            prod[i][j]=0;
            for(k=0;k<matrixSize;k++)
                prod[i][j] = prod[i][j]+mat1[i][k] * mat2[k][j];
        }
    }
}

/** ============================================================================
 *  @func   PrepareMSGArray
 *
 *  @desc   This function retrieve the elements from Matrixes and 
 *          allocate them into the message for sending.
 *
 *  @modif  None
 *  ============================================================================
 */
Uint32 PrepareMSGArray(Uint32 CurrMatrix[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX], Uint32 MatrixSize, Uint16 *Row, Uint16 *Colume, Uint32 PackageEntries, ControlMsg *msg){
    Uint32 ProcessedEntries = 0;
    
    while((ProcessedEntries<PackageEntries)&&((*Row)!=MatrixSize)){
        msg->int_arg[ProcessedEntries] = CurrMatrix[(*Row)][(*Colume)];
        (*Colume)++;
        ProcessedEntries++;
        
        if((*Colume)==MatrixSize){
            (*Colume)=0;
            (*Row)++;
        }
    }
    return ProcessedEntries;
}

/** ============================================================================
 *  @func   PrepareMsg
 *
 *  @desc   This function set the necessary setting for msg.
 *          Mainly used by SendMatrix to set the number of sending elements.
 *
 *  @modif  None
 *  ============================================================================
 */
void PrepareMsg(ControlMsg *msg, Uint16 CurrEntries){
    // Edit the message and send it over
    
    /* Increment the sequenceNumber for next received message */
    info->sequenceNumber++;
    /* Make sure that sequenceNumber stays within the range of iterations */
    if (info->sequenceNumber == MSGQ_INTERNALIDSSTART)
    {
        info->sequenceNumber = 0;
    }
    MSGQ_setMsgId((MSGQ_Msg) msg, info->sequenceNumber);
    MSGQ_setSrcQueue((MSGQ_Msg) msg, info->localMsgq);
    

    msg->PackageEntries = CurrEntries;
}

/** ============================================================================
 *  @func   SendMatrixMsg
 *
 *  @desc   This function will send the given message to ARM.
 *
 *  @modif  None
 *  ============================================================================
 */
void SendMatrixMsg(ControlMsg *msg){
    Int status = MSGQ_put(info->locatedMsgq, (MSGQ_Msg) msg);
    if (status != SYS_OK)
    {
        MSGQ_free ((MSGQ_Msg) msg);
        SET_FAILURE_REASON(status);
    }
}

/** ============================================================================
 *  @func   WaitForEnable
 *
 *  @desc   This function wait for dummy Message from ARM. It should work with
 *          EnableARM_Sending @ ARM.
 *
 *  @usage  Use this function to ensure the communication flow.
 *          The communication flow need to follow send+Receive pattern
 *          to ensure the DSP/ARM will never overwrite the previous message,
 *          Which will cause kernal error.
 *
 *  @modif  None
 *  ============================================================================
 */
void WaitForEnable(ControlMsg *msg){
    WaitMatrixMsg(msg);
}

/** ============================================================================
 *  @func   EnableDSP_Sending
 *
 *  @desc   This function send dummy Message to ARM. It should work with
 *          WaitForEnable @ ARM.
 *
 *  @usage  Use this function to ensure the communication flow.
 *          The communication flow need to follow send+Receive pattern
 *          to ensure the DSP/ARM will never overwrite the previous message,
 *          Which will cause kernal error.
 *
 *  @modif  None
 *  ============================================================================
 */
void EnableARM_Sending(ControlMsg *msg){
    Int status;
    // Edit the message and send it over
    /* Increment the sequenceNumber for next received message */
    info->sequenceNumber++;
    /* Make sure that sequenceNumber stays within the range of iterations */
    if (info->sequenceNumber == MSGQ_INTERNALIDSSTART)
    {
        info->sequenceNumber = 0;
    }
    MSGQ_setMsgId((MSGQ_Msg) msg, info->sequenceNumber);
    MSGQ_setSrcQueue((MSGQ_Msg) msg, info->localMsgq);
    
    status = MSGQ_put(info->locatedMsgq, (MSGQ_Msg) msg);
    if (status != SYS_OK)
    {
        MSGQ_free ((MSGQ_Msg) msg);
        SET_FAILURE_REASON(status);
    }
}

/** ============================================================================
 *  @func   WaitAcknowledgement
 *
 *  @desc   This function will wait for the acknowledgement from ARM.
 *          It is used by SendMatrix to ensure the ARM received the elements of Matrix.
 *
 *  @modif  None
 *  ============================================================================
 */
void WaitAcknowledgement(ControlMsg *msg, Uint32 ExpectedRequest){
    Uint32 Information;
    Int status = MSGQ_get(info->localMsgq, (MSGQ_Msg*) &msg, SYS_FOREVER);
    
    if (status == SYS_OK)
    {
        //memcpy(&Information, msg->arg1, sizeof(Uint32));    
        Information = msg->PackageEntries;
        if(Information!=ExpectedRequest){
            //SYS_sprintf("Mismatch occured! Expected Request:%d \tReceived Request:%d\n", ExpectedRequest, Information);
        }
    }
    else
    {
        MSGQ_free ((MSGQ_Msg) msg);
        SET_FAILURE_REASON(status);
    }
}

/** ============================================================================
 *  @func   SendMatrix
 *
 *  @desc   This function will send a Matrix to ARM with given parameter.
 *
 *  @modif  None
 *  ============================================================================
 */
void SendMatrix(ControlMsg *msg, Uint32 CurrMatrix[SIZE_MAX_MATRIX][SIZE_MAX_MATRIX], Uint32 MatrixSize, Uint32 PackageEntries){
    Uint32 RemainingEnties = MatrixSize*MatrixSize;
    Uint32 CurrEntries;
    Uint32 ExpectedRequest;
    Uint16 Row=0, Colume=0;
    
    while(RemainingEnties){
        CurrEntries = PrepareMSGArray(CurrMatrix, MatrixSize, &Row, &Colume, PackageEntries, msg);
        PrepareMsg(msg, CurrEntries);
        SendMatrixMsg(msg);
        
        RemainingEnties -= CurrEntries;
        ExpectedRequest = RemainingEnties;
        
        WaitAcknowledgement(msg, ExpectedRequest);
    }
}

/** ============================================================================
 *  @func   ReceiveCommandMessage
 *
 *  @desc   This function will receive a command to ARM and return it to the caller
 *          for further actions.
 *
 *  @modif  None
 *  ============================================================================
 */
Uint16 ReceiveCommandMessage(ControlMsg* msg){
    Uint16 Command;
    Int status = MSGQ_get(info->localMsgq, (MSGQ_Msg*) &msg, SYS_FOREVER);
    
    if (status == SYS_OK)
    {
        Command = msg->command;
    }
    else
    {
        MSGQ_free ((MSGQ_Msg) msg);
        SET_FAILURE_REASON(status);
    }
    return Command;
}

/** ============================================================================
 *  @func   TSKMESSAGE_execute
 *
 *  @desc   Execute phase function for the TSKMESSAGE application. Application
 *          receives a message, verifies the id and executes the DSP processing.
 *
 *  @modif  None.
 *  ============================================================================
 */
Int TSKMESSAGE_execute(TSKMESSAGE_TransferInfo* info)
{
    Int status = SYS_OK;
    ControlMsg* msg = &Gmsg;
    Uint32 PackageEntries = SIZE_PACKAGE_ENTRIES;
    
    status = MSGQ_alloc(SAMPLE_POOL_ID, (MSGQ_Msg*) &msg, APP_BUFFER_SIZE);

    if (status == SYS_OK)
    {
        MSGQ_setMsgId((MSGQ_Msg) msg, info->sequenceNumber);
        MSGQ_setSrcQueue((MSGQ_Msg) msg, info->localMsgq);
        msg->command = 0x01;
        SYS_sprintf(msg->arg1, "DSP is awake!");

        status = MSGQ_put(info->locatedMsgq, (MSGQ_Msg) msg);
        if (status != SYS_OK)
        {
            /* Must free the message */
            MSGQ_free ((MSGQ_Msg) msg);
            SET_FAILURE_REASON(status);
        }
    }
    else
    {
        SET_FAILURE_REASON(status);
    }
    
    while(ReceiveCommandMessage(msg)==COMMAND_CONTINUE){
        
        EnableARM_Sending(msg);
        
        ReceiveMatrix(mat1, info->matrixSize, msg);
        ReceiveMatrix(mat2, info->matrixSize, msg);
        //PackageEntries = msg->PackageEntries;
        
        WaitForEnable(msg);
        matMult(mat1, mat2, prod, info->matrixSize);
        SendMatrix(msg, prod, info->matrixSize, PackageEntries);
        
        EnableARM_Sending(msg);
        //WaitForEnable(msg);
    }
    
    return status;
}





/** ============================================================================
 *  @func   TSKMESSAGE_delete
 *
 *  @desc   Delete phase function for the TSKMESSAGE application. It deallocates
 *          all the resources of allocated during create phase of the
 *          application.
 *
 *  @modif  None.
 *  ============================================================================
 */
Int TSKMESSAGE_delete(TSKMESSAGE_TransferInfo* info)
{
    Int status = SYS_OK;
    Int tmpStatus = SYS_OK;
    Bool freeStatus = FALSE;

    /* Release the located message queue */
    if (info->locatedMsgq != MSGQ_INVALIDMSGQ)
    {
        status = MSGQ_release(info->locatedMsgq);
        if (status != SYS_OK)
        {
            SET_FAILURE_REASON(status);
        }
    }

     /* Reset the error handler before deleting the MSGQ that receives */
     /* the error messages.                                            */
    MSGQ_setErrorHandler(MSGQ_INVALIDMSGQ, POOL_INVALIDID);

    /* Delete the message queue */
    if (info->localMsgq != MSGQ_INVALIDMSGQ)
    {
        tmpStatus = MSGQ_close(info->localMsgq);
        if ((status == SYS_OK) && (tmpStatus != SYS_OK))
        {
            status = tmpStatus;
            SET_FAILURE_REASON(status);
        }
    }

    /* Free the info structure */
    freeStatus = MEM_free(DSPLINK_SEGID, info, sizeof(TSKMESSAGE_TransferInfo));
    if ((status == SYS_OK) && (freeStatus != TRUE))
    {
        status = SYS_EFREE;
        SET_FAILURE_REASON(status);
    }
    return status;
}


#if defined (__cplusplus)
}
#endif /* defined (__cplusplus) */
